#include "Stepper.hpp"
#include "log.h"
#define LOGTAG "Stepper"
Stepper::Stepper(uint32_t oneCirclePulseNum, long minStep, long maxStep)
{
    this->oneCirclePulseNum = oneCirclePulseNum;
    this->limitMinStep = minStep;
    this->limitMaxStep = maxStep;
    StepperDriverInit(&this->driver);
}

void Stepper::attachDirPin(GPIO_TypeDef *gpio, uint16_t pinBit, uint16_t defaultVoltage)
{
    StepperDriverAttachDirPin(&this->driver, gpio, pinBit, defaultVoltage);
}

void Stepper::attachEnPin(GPIO_TypeDef *gpio, uint16_t pinBit, uint16_t defaultVoltage)
{
    StepperDriverAttachEnPin(&this->driver, gpio, pinBit, defaultVoltage);
}

void Stepper::attachPulsePin(GPIO_TypeDef *gpio, uint16_t pinBit,TIM_HandleTypeDef *htim, uint8_t channel)
{
    StepperDriverAttachPulsePin(&this->driver, gpio, pinBit, htim, channel);
}

void Stepper::attachLimitSW1(GPIO_TypeDef *gpio, uint16_t pinBit, uint16_t defaultVoltage)
{
    this->limitsw1.attachPin(gpio, pinBit, defaultVoltage);
}
void Stepper::attachLimitSW2(GPIO_TypeDef *gpio, uint16_t pinBit, uint16_t defaultVoltage)
{
    this->limitsw2.attachPin(gpio, pinBit, defaultVoltage);
}

void Stepper::setDriverEnableStatus(uint32_t status)
{
    StepperDriverSetEnableStatus(&this->driver, status);
}

uint32_t Stepper::getDriverEnableStatus(void)
{
    return StepperDriverGetEnableStatus(&this->driver);
}

long Stepper::angleToStep(double angle)
{
    return (long)(angle / 360.0f * (this->limitMaxStep - this->limitMinStep));
}
double Stepper::stepToAngle(long long step)
{
    return (double)(step / (double)(this->limitMaxStep - this->limitMinStep)) * 360.0f;
}

/**
 * @brief 设置转速
 * @param speed 转速(r/s)
 */
void Stepper::setSpeed(double speed)
{
    StepperDriverSetFrequnce(&this->driver, speed * this->oneCirclePulseNum);
}

double Stepper::getSpeed(void)
{
    return (double)StepperDriverGetFrequnce(&this->driver) / this->oneCirclePulseNum;
}

long long Stepper::getStep(void)
{
    return StepperDriverGetStep(&this->driver);
}

double Stepper::getAngle(void)
{
    return this->stepToAngle(this->getStep());
}

void Stepper::moveStep(long step)
{
    long long stepCnt;
    if (this->limitStepState)
    {
        stepCnt = this->getStep();
        if (step + stepCnt > this->limitMaxStep)
        {
            step = this->limitMaxStep - stepCnt;
        }
        else if (step + stepCnt < this->limitMinStep)
        {
            step = this->limitMinStep - stepCnt;
        }
    }
    StepperDriverMoveSteps(&this->driver, step);
}

void Stepper::moveToStep(long long stepPos)
{
    if (this->limitStepState)
    {
        if (stepPos > this->limitMaxStep)
        {
            stepPos = this->limitMaxStep;
        }
        else if (stepPos < this->limitMinStep)
        {
            stepPos = this->limitMinStep;
        }
    }
    StepperDriverMovePos(&this->driver, stepPos);
}

void Stepper::rotateAngle(float angle)
{
    this->moveStep(this->angleToStep(angle));
}

void Stepper::rotateToAngle(float targetAngle)
{
    long step = this->angleToStep(targetAngle);
    this->moveToStep(step);
}

void Stepper::stop(void)
{
    StepperDriverStopMove(&this->driver);
}

StepperDriverState_t Stepper::getMoveState(void)
{
    if (this->driver.curDir == 0) return STEPPER_DRIVER_OK;
    else return STEPPER_DRIVER_BUSY;
}

void Stepper::limitStepEnable(void)
{
    this->limitStepState = true;
}

void Stepper::limitStepDisable(void)
{
    this->limitStepState = false;
}

StepperDriverState_t Stepper::reset(int dir)
{
    if ((this->limitsw1.gpio == NULL && dir < 0) || (this->limitsw2.gpio == NULL && dir > 0))
        return STEPPER_DRIVER_ERROR;
    if (dir >= 0)
    {
        if (this->limitsw2.getState()) return STEPPER_DRIVER_OK;
        this->limitStepDisable();
        this->moveStep(this->limitMaxStep - this->limitMinStep);
    }
    else
    {
        if (this->limitsw1.getState()) return STEPPER_DRIVER_OK;
        this->limitStepDisable();
        this->moveStep(this->limitMinStep - this->limitMaxStep);
    }
	return STEPPER_DRIVER_BUSY;
}

void Stepper::extiCallback(uint16_t GPIO_Pin)
{
    if (this->limitStepState != 0)
        return;

    if (GPIO_Pin == this->limitsw1.pinBit)
    {
        if (this->limitsw1.getState() == 0 || this->driver.curDir > 0) return;
        this->stop();
        this->driver.stepCnt = 0;
    }
    else if (GPIO_Pin == this->limitsw2.pinBit)
    {
        if (this->limitsw2.getState() == 0 || this->driver.curDir < 0) return;
        this->stop();
        this->driver.stepCnt = this->limitMaxStep;
    }
    else
    {
        return;
    }
    this->limitStepEnable();
    LOG_INFO(LOGTAG, "EXTI trigger, reset cplt\n");
}
